﻿#include "AircraftSimulation.h"
using namespace std;
using namespace ASL;

// 构造函数
F16Simulation::F16Simulation()
{
}

// 析构函数
F16Simulation::~F16Simulation()
{
}

// 准备函数
void F16Simulation::setup(void)
{
    // 仿真求解配置
    this->startTime = 0.0;
    this->stopTime = 100.0;
    this->stepSize = 0.005;
    this->currentTime = 0.0;
    this->currentCount = 0;

    // 设置初始运动参数
    this->cDOFModel.parameter.rLLA(AXIS_LON) = 122.0;
    this->cDOFModel.parameter.rLLA(AXIS_LAT) = 47.0;
    this->cDOFModel.parameter.rLLA(AXIS_ALT) = 5.5 * FT_TO_M;

    this->cDOFModel.parameter.vBody(AXIS_X) = 0;
    this->cDOFModel.parameter.vBody(AXIS_Y) = 0;
    this->cDOFModel.parameter.vBody(AXIS_Z) = 0;

    this->cDOFModel.parameter.eular(AXIS_X) = 0;
    this->cDOFModel.parameter.eular(AXIS_Y) = 0;
    this->cDOFModel.parameter.eular(AXIS_Z) = 0;

    this->cDOFModel.parameter.omega(AXIS_X) = 0;
    this->cDOFModel.parameter.omega(AXIS_Y) = 0;
    this->cDOFModel.parameter.omega(AXIS_Z) = 0;

    // 设置质量特性参数
    this->cMassModel.setup(AIRCRAFT_F16);

    // 设置起落架布局参数
    this->cLandingGearModel.setup(AIRCRAFT_F16);
    this->cLandingGearModel.parameter.initialPosition = 1.0;
    this->cLandingGearModel.parameter.steerAngle = 80.0;

    // 操纵舵面机构参数
    this->cAileronActutor.parameter.k = 20.0;
    this->cAileronActutor.parameter.positionRange[AXIS_MIN] = -20.0;
    this->cAileronActutor.parameter.positionRange[AXIS_MAX] = +20.0;
    this->cAileronActutor.parameter.rateRange[AXIS_MIN] = -80.0;
    this->cAileronActutor.parameter.rateRange[AXIS_MAX] = +80.0;

    this->cRudderActutor.parameter.k = 20.0;
    this->cRudderActutor.parameter.positionRange[AXIS_MIN] = -30.0;
    this->cRudderActutor.parameter.positionRange[AXIS_MAX] = +30.0;
    this->cRudderActutor.parameter.rateRange[AXIS_MIN] = -80.0;
    this->cRudderActutor.parameter.rateRange[AXIS_MAX] = +80.0;

    this->cElevatorActutor.parameter.k = 20.0;
    this->cElevatorActutor.parameter.positionRange[AXIS_MIN] = -25.0;
    this->cElevatorActutor.parameter.positionRange[AXIS_MAX] = +25.0;
    this->cElevatorActutor.parameter.rateRange[AXIS_MIN] = -80.0;
    this->cElevatorActutor.parameter.rateRange[AXIS_MAX] = +80.0;

    this->cLeadingActutor.parameter.k = 7.0;
    this->cLeadingActutor.parameter.positionRange[AXIS_MIN] = -2.0;
    this->cLeadingActutor.parameter.positionRange[AXIS_MAX] = +25.0;
    this->cLeadingActutor.parameter.rateRange[AXIS_MIN] = -25.0;
    this->cLeadingActutor.parameter.rateRange[AXIS_MAX] = +25.0;

    this->cTrailingActutor.parameter.k = 7.0;
    this->cTrailingActutor.parameter.positionRange[AXIS_MIN] = -2.0;
    this->cTrailingActutor.parameter.positionRange[AXIS_MAX] = +20.0;
    this->cTrailingActutor.parameter.rateRange[AXIS_MIN] = -25.0;
    this->cTrailingActutor.parameter.rateRange[AXIS_MAX] = +25.0;

    this->cSpeedActutor.parameter.k = 7.0;
    this->cSpeedActutor.parameter.positionRange[AXIS_MIN] = 0.0;
    this->cSpeedActutor.parameter.positionRange[AXIS_MAX] = 60.0;
    this->cSpeedActutor.parameter.rateRange[AXIS_MIN] = -25.0;
    this->cSpeedActutor.parameter.rateRange[AXIS_MAX] = +25.0;

    this->cFlapActutor.parameter.k = 7.0;
    this->cFlapActutor.parameter.positionRange[AXIS_MIN] = -20.0;
    this->cFlapActutor.parameter.positionRange[AXIS_MAX] = +20.0;
    this->cFlapActutor.parameter.rateRange[AXIS_MIN] = -25.0;
    this->cFlapActutor.parameter.rateRange[AXIS_MAX] = +25.0;
}

// 初始化函数
void F16Simulation::initialize(string resourceDir)
{
    // 飞行运动学模型
    this->cDOFModel.initialize(this->stepSize);

    // 大气环境模型
    this->cAtmosModel.input.altitude = cDOFModel.parameter.rLLA(AXIS_ALT);
    this->cAtmosModel.initialize();

    // 质量特性模型
    this->cMassModel.input.fuelExpendedL = 0.0;
    this->cMassModel.input.fuelExpendedR = 0.0;
    this->cMassModel.initialize();

    // 气动特性模型
    this->cAeroModel.initialize(resourceDir + "/F16");

    // 动力系统模型
    this->cTurbineModel.initialize(resourceDir + "/F100-PW-229", stepSize);
    this->cTurbineModel.starter(true);

    // 控制系统模型
    this->cControlModel.initialize(this->stepSize);

    // 起落架模型
    this->cLandingGearModel.initialize(this->stepSize);

    // 执行机构模型
    this->cAileronActutor.initialize(this->stepSize);
    this->cRudderActutor.initialize(this->stepSize);
    this->cElevatorActutor.initialize(this->stepSize);
    this->cLeadingActutor.initialize(this->stepSize);
    this->cTrailingActutor.initialize(this->stepSize);
    this->cSpeedActutor.initialize(this->stepSize);
    this->cFlapActutor.initialize(this->stepSize);

    // 创建数据记录文件
    this->cStream.open("Example8.txt");
    this->cStream.setf(ios::showpos);
    this->cStream.setf(ios::fixed);
    this->cStream.precision(8);

    // 打印数据标题文本
    this->cStream << "Time(s)\t";
    this->cStream << "Longitude(deg)\tLatitude(deg)\tAltitude(m)\t";
    this->cStream << "V-North(m/s)\tV-Up(m/s)\tV-East(m/s)\t";
    this->cStream << "A-Body-X(m/s^2)\tA-Body-Y(m/s^2)\tA-Body-Z(m/s^2)\t";
    this->cStream << "Gamma(deg)\tPsi(deg)\tZeta(deg)\t";
    this->cStream << "Omega-X(deg/s)\tOmega-Y(deg/s)\tOmega-Z(deg/s)\t";
    this->cStream << "Mach(-)\tAlpha(deg)\tBeta(deg)\t";
    this->cStream << "Gear-Force-X(N)\tGear-Force-Y(N)\tGear-Force-Z(N)\t";
    this->cStream << "Gear-Torque-X(N*m)\tGear-Torque-Y(N*m)\tGear-Torque-Z(N*m)\t";
    this->cStream << "Engine-Force-X(N)\tEngine-Force-Y(N)\tEngine-Force-Z(N)\t";
    this->cStream << "Engine-Torque-X(N*m)\tEngine-Torque-Y(N*m)\tEngine-Torque-Z(N*m)\t";
    this->cStream << "Aero-Force-X(N)\tAero-Force-Y(N)\tAero-Force-Z(N)\t";
    this->cStream << "Aero-Torque-X(N*m)\tAero-Torque-Y(N*m)\tAero-Torque-Z(N*m)\t";
    this->cStream << "Aileron(deg)\tRudder(deg)\tElevator(deg)\t";
    this->cStream << "LeadingEdgeFlap(deg)\tTrailingEdgeFlap(deg)\tSpeedBrake(deg)\t";
    this->cStream << "Flaperon(deg)\t";
    this->cStream << endl;
}

// 单步运行函数
void F16Simulation::step(void)
{
    double Q, v = 0.0, mach = 0.0;
    double alpha = 0.0, beta = 0.0;
    Vector accel(3), force(3), torque(3), g(3);

    // 仿真运算循环
    while (currentTime < stopTime)
    {
        // 更新仿真时间
        this->currentTime = this->currentCount * this->stepSize;

        // 记录仿真数据
        this->cStream << currentTime << "\t";
        this->cStream << cDOFModel.output.rLLA << cDOFModel.output.vNav << cDOFModel.output.linearAccel;
        this->cStream << cDOFModel.output.eular << cDOFModel.output.omegaR;
        this->cStream << mach << "\t" << alpha << "\t" << beta << "\t";
        this->cStream << cLandingGearModel.output.force << cLandingGearModel.output.torque;
        this->cStream << cTurbineModel.output.force << cTurbineModel.output.torque;
        this->cStream << cAeroModel.output.force << cAeroModel.output.torque;
        this->cStream << cControlModel.output.aileron << "\t" << cControlModel.output.rudder << "\t" << cControlModel.output.elevator << "\t";
        this->cStream << cControlModel.output.leadingEdgeFlap << "\t" << cControlModel.output.trailingEdgeFlap << "\t" << cControlModel.output.speedBrake << "\t";
        this->cStream << cControlModel.output.flaperon << "\t";
        this->cStream << endl;

        // 飞行运动学模型
        force = cAeroModel.output.force + cLandingGearModel.output.force + cTurbineModel.output.force;
        torque = cAeroModel.output.torque + cLandingGearModel.output.torque + cTurbineModel.output.torque;
        g = Geodesy::gravity(cDOFModel.output.rLLA(0), cDOFModel.output.rLLA(1), cDOFModel.output.rLLA(2));
        g = cDOFModel.output.cN2B * g * cMassModel.output.mass;

        this->cDOFModel.input.force = force + g;
        this->cDOFModel.input.torque = torque;
        this->cDOFModel.input.mass = cMassModel.output.mass;
        this->cDOFModel.input.inertia = cMassModel.output.inertia;
        this->cDOFModel.step();

        // 大气环境模型
        this->cAtmosModel.input.altitude = cDOFModel.output.rLLA(AXIS_ALT);
        this->cAtmosModel.step();

        // 飞行参数解算
        alpha = Coordinate::alpha(cDOFModel.output.vBody(AXIS_X), cDOFModel.output.vBody(AXIS_Y), cDOFModel.output.vBody(AXIS_Z));
        beta = Coordinate::beta(cDOFModel.output.vBody(AXIS_X), cDOFModel.output.vBody(AXIS_Y), cDOFModel.output.vBody(AXIS_Z));
        v = Vector::norm(cDOFModel.output.vBody);
        mach = v / cAtmosModel.output.speed;
        Q = 0.5 * cAtmosModel.output.density * v * v;
        // accel = cDOFModel.output.aBody * (1.0 / G_EARTH);
        accel = force * (1.0 / cMassModel.output.mass) * (1.0 / G_EARTH);
        accel(AXIS_Y) += cos(cDOFModel.output.eular(AXIS_X) / RAD_TO_DEG) * cos(cDOFModel.output.eular(AXIS_Z) / RAD_TO_DEG);

        // 更新操纵指令
        this->autopilot();

        // 质量特性模型
        this->cMassModel.input.fuelExpendedL = cTurbineModel.output.fuelExpended * 0.5;
        this->cMassModel.input.fuelExpendedR = cTurbineModel.output.fuelExpended * 0.5;
        this->cMassModel.step();

        // 气动特性模型
        this->cAeroModel.input.vTAS = v;
        this->cAeroModel.input.mach = mach;
        this->cAeroModel.input.alpha = alpha;
        this->cAeroModel.input.beta = beta;
        this->cAeroModel.input.dynamicPressure = Q;
        this->cAeroModel.input.omega = cDOFModel.output.omegaR;
        this->cAeroModel.input.altitudeGL = cDOFModel.output.rLLA(AXIS_ALT);
        this->cAeroModel.input.CG = cMassModel.output.CG;

        this->cAeroModel.input.aileron = cAileronActutor.output.y;
        this->cAeroModel.input.rudder = cRudderActutor.output.y;
        this->cAeroModel.input.elevator = cElevatorActutor.output.y;
        this->cAeroModel.input.leadingEdgeFlap = cLeadingActutor.output.y;
        this->cAeroModel.input.trailingEdgeFlap = cTrailingActutor.output.y;
        this->cAeroModel.input.speedBrake = cSpeedActutor.output.y;
        this->cAeroModel.input.flaperon = cFlapActutor.output.y;
        this->cAeroModel.input.landingGear = cLandingGearModel.output.gearPosition;

        this->cAeroModel.step();

        // 动力系统模型
        // this->cTurbineModel.input.throttle = 0;
        this->cTurbineModel.input.mach = mach;
        this->cTurbineModel.input.temperature = cAtmosModel.output.temperature;
        this->cTurbineModel.input.dynamicPressure = Q;
        this->cTurbineModel.input.densityRatio = cAtmosModel.output.density / Geodesy::density(0.0);
        this->cTurbineModel.input.densityAltitude = MathOperation::densityAltitude(cDOFModel.output.rLLA(2));
        this->cTurbineModel.input.CG = cMassModel.output.CG;
        this->cTurbineModel.step();

        // 起落架模型
        // this->cLandingGearModel.input.gearCommand = 0;
        // this->cLandingGearModel.input.steerCommand = 0;
        // this->cLandingGearModel.input.brakeCommand = 0;
        this->cLandingGearModel.input.vNav = cDOFModel.output.vNav;
        this->cLandingGearModel.input.eular = cDOFModel.output.eular;
        this->cLandingGearModel.input.omega = cDOFModel.output.omegaR;
        this->cLandingGearModel.input.altitudeGL = cDOFModel.output.rLLA(AXIS_ALT);
        this->cLandingGearModel.input.CG = cMassModel.output.CG;
        this->cLandingGearModel.step();

        // 控制系统模型
        this->cControlModel.input.mach = mach;
        this->cControlModel.input.alpha = alpha;
        this->cControlModel.input.beta = beta;
        this->cControlModel.input.eular = cDOFModel.output.eular;
        this->cControlModel.input.omega = cDOFModel.output.omegaR;
        this->cControlModel.input.vNav = cDOFModel.output.vNav;
        this->cControlModel.input.vCAS = MathOperation::vcalibratedAirSpeed(mach, cDOFModel.output.rLLA(2));
        this->cControlModel.input.g = accel;

        // this->cControlModel.input.aileronCommand = 0;
        // this->cControlModel.input.rudderCommand = 0;
        // this->cControlModel.input.elevatorCommand = 0;
        // this->cControlModel.input.speedBrakeCommand = 0;
        this->cControlModel.input.landingGear = cLandingGearModel.output.gearPosition;
        this->cControlModel.input.wheelReaction = cLandingGearModel.output.wheelReaction;

        this->cControlModel.step();

        // 位置伺服执行机构
        this->cAileronActutor.input.u = cControlModel.output.aileron;
        this->cAileronActutor.step();

        this->cRudderActutor.input.u = cControlModel.output.rudder;
        this->cRudderActutor.step();

        this->cElevatorActutor.input.u = cControlModel.output.elevator;
        this->cElevatorActutor.step();

        this->cLeadingActutor.input.u = cControlModel.output.leadingEdgeFlap;
        this->cLeadingActutor.step();

        this->cTrailingActutor.input.u = cControlModel.output.trailingEdgeFlap;
        this->cTrailingActutor.step();

        this->cSpeedActutor.input.u = cControlModel.output.speedBrake;
        this->cSpeedActutor.step();

        this->cFlapActutor.input.u = cControlModel.output.flaperon;
        this->cFlapActutor.step();

        // 更新周期计数器
        this->currentCount++;
    }

    // 关闭文件流
    this->cStream.close();
}

// 操纵指令函数
void F16Simulation::autopilot(void)
{
    double v;

    if (this->currentTime < 5.0)
    {
        this->cLandingGearModel.input.gearCommand = 1.0;
        this->cTurbineModel.input.throttle = 0.0;
        this->cControlModel.input.aileronCommand = 0;
        this->cControlModel.input.rudderCommand = 0;
        this->cControlModel.input.elevatorCommand = 0;
        this->cControlModel.input.speedBrakeCommand = 0;
    }
    else if (this->currentTime < 15.0)
    {
        this->cTurbineModel.input.throttle = 0.5;
    }
    else if (this->currentTime < 22.5)
    {
        this->cTurbineModel.input.throttle = 0.5;
        this->cControlModel.input.elevatorCommand = -0.05;
    }
    else if (this->currentTime < 50.0)
    {
        this->cTurbineModel.input.throttle = 1.0;
        this->cControlModel.input.elevatorCommand = -0.05;
    }
    else
    {
        this->cLandingGearModel.input.gearCommand = 0.0;
        this->cTurbineModel.input.throttle = 1.0;
        this->cControlModel.input.elevatorCommand = -0.05;
    }
}

// 构造函数
B737Simulation::B737Simulation()
{
}

// 析构函数
B737Simulation::~B737Simulation()
{
}

// 准备函数
void B737Simulation::setup(void)
{
    // 仿真求解配置
    startTime = 0.0;
    stopTime = 100.0;
    stepSize = 0.005;
    currentTime = 0.0;
    currentCount = 0;

    // 设置初始运动参数
    this->cDOFModel.parameter.rLLA(AXIS_LON) = 122.0;
    this->cDOFModel.parameter.rLLA(AXIS_LAT) = 47.0;
    this->cDOFModel.parameter.rLLA(AXIS_ALT) = 49.0 * IN_TO_M;

    this->cDOFModel.parameter.vBody(AXIS_X) = 0;
    this->cDOFModel.parameter.vBody(AXIS_Y) = 0;
    this->cDOFModel.parameter.vBody(AXIS_Z) = 0;

    this->cDOFModel.parameter.eular(AXIS_X) = 0;
    this->cDOFModel.parameter.eular(AXIS_Y) = 135.0;
    this->cDOFModel.parameter.eular(AXIS_Z) = 0;

    this->cDOFModel.parameter.omega(AXIS_X) = 0;
    this->cDOFModel.parameter.omega(AXIS_Y) = 0;
    this->cDOFModel.parameter.omega(AXIS_Z) = 0;

    // 设置质量特性参数
    this->cMassModel.setup(AIRCRAFT_B737);

    // 设置起落架布局参数
    this->cLandingGearModel.setup(AIRCRAFT_B737);
    this->cLandingGearModel.parameter.cGearN.linearBand = 0.1;
    this->cLandingGearModel.parameter.cGearL.linearBand = 0.1;
    this->cLandingGearModel.parameter.cGearR.linearBand = 0.1;

    // 设置发动机布局参数 L
    this->cTurbineLModel.parameter.thrustMaximum = 20000.0 * LBF_TO_N;
    this->cTurbineLModel.parameter.bleed = 0.04;
    this->cTurbineLModel.parameter.location(AXIS_X) = 540.0 * IN_TO_M;
    this->cTurbineLModel.parameter.location(AXIS_Y) = -40.0 * IN_TO_M;
    this->cTurbineLModel.parameter.location(AXIS_Z) = 193.0 * IN_TO_M;

    // 设置发动机布局参数 R
    this->cTurbineRModel.parameter.thrustMaximum = 20000.0 * LBF_TO_N;
    this->cTurbineRModel.parameter.bleed = 0.04;
    this->cTurbineRModel.parameter.location(AXIS_X) = 540.0 * IN_TO_M;
    this->cTurbineRModel.parameter.location(AXIS_Y) = -40.0 * IN_TO_M;
    this->cTurbineRModel.parameter.location(AXIS_Z) = -193.0 * IN_TO_M;

    // 设置操纵舵面机构参数
    this->cAileronActutor.parameter.k = 20.0;
    this->cAileronActutor.parameter.positionRange[AXIS_MIN] = -20.0;
    this->cAileronActutor.parameter.positionRange[AXIS_MAX] = +20.0;
    this->cAileronActutor.parameter.rateRange[AXIS_MIN] = -80.0;
    this->cAileronActutor.parameter.rateRange[AXIS_MAX] = +80.0;

    this->cRudderActutor.parameter.k = 20.0;
    this->cRudderActutor.parameter.positionRange[AXIS_MIN] = -20.0;
    this->cRudderActutor.parameter.positionRange[AXIS_MAX] = +20.0;
    this->cRudderActutor.parameter.rateRange[AXIS_MIN] = -80.0;
    this->cRudderActutor.parameter.rateRange[AXIS_MAX] = +80.0;

    this->cElevatorActutor.parameter.k = 20.0;
    this->cElevatorActutor.parameter.positionRange[AXIS_MIN] = -15.0;
    this->cElevatorActutor.parameter.positionRange[AXIS_MAX] = +15.0;
    this->cElevatorActutor.parameter.rateRange[AXIS_MIN] = -80.0;
    this->cElevatorActutor.parameter.rateRange[AXIS_MAX] = +80.0;

    this->cFlapActutor.parameter.k = 7.0;
    this->cFlapActutor.parameter.positionRange[AXIS_MIN] = 0.0;
    this->cFlapActutor.parameter.positionRange[AXIS_MAX] = 1.0;
    this->cFlapActutor.parameter.rateRange[AXIS_MIN] = -25.0;
    this->cFlapActutor.parameter.rateRange[AXIS_MAX] = +25.0;

    this->cSpeedActutor.parameter.k = 7.0;
    this->cSpeedActutor.parameter.positionRange[AXIS_MIN] = 0.0;
    this->cSpeedActutor.parameter.positionRange[AXIS_MAX] = 1.0;
    this->cSpeedActutor.parameter.rateRange[AXIS_MIN] = -25.0;
    this->cSpeedActutor.parameter.rateRange[AXIS_MAX] = +25.0;

    this->cSpoilerActutor.parameter.k = 7.0;
    this->cSpoilerActutor.parameter.positionRange[AXIS_MIN] = 0.0;
    this->cSpoilerActutor.parameter.positionRange[AXIS_MAX] = 1.0;
    this->cSpoilerActutor.parameter.rateRange[AXIS_MIN] = -25.0;
    this->cSpoilerActutor.parameter.rateRange[AXIS_MAX] = +25.0;

    // 创建本地数据文件
    this->cStream.open("Example8.txt");
    this->cStream.setf(ios::showpos);
    this->cStream.setf(ios::fixed);
    this->cStream.precision(8);

    // 打印数据标题文本
    this->cStream << "Time(s)\t";
    this->cStream << "Longitude(deg)\tLatitude(deg)\tAltitude(m)\t";
    this->cStream << "V-North(m/s)\tV-Up(m/s)\tV-East(m/s)\t";
    this->cStream << "A-Body-X(m/s^2)\tA-Body-Y(m/s^2)\tA-Body-Z(m/s^2)\t";
    this->cStream << "Gamma(deg)\tPsi(deg)\tZeta(deg)\t";
    this->cStream << "Omega-X(deg/s)\tOmega-Y(deg/s)\tOmega-Z(deg/s)\t";
    this->cStream << "Mach(-)\tAlpha(deg)\tBeta(deg)\t";
    this->cStream << "Gear-Force-X(N)\tGear-Force-Y(N)\tGear-Force-Z(N)\t";
    this->cStream << "Gear-Torque-X(N*m)\tGear-Torque-Y(N*m)\tGear-Torque-Z(N*m)\t";
    this->cStream << "Engine-Force-X(N)\tEngine-Force-Y(N)\tEngine-Force-Z(N)\t";
    this->cStream << "Engine-Torque-X(N*m)\tEngine-Torque-Y(N*m)\tEngine-Torque-Z(N*m)\t";
    this->cStream << "Aero-Force-X(N)\tAero-Force-Y(N)\tAero-Force-Z(N)\t";
    this->cStream << "Aero-Torque-X(N*m)\tAero-Torque-Y(N*m)\tAero-Torque-Z(N*m)\t";
    this->cStream << "Aileron(deg)\tRudder(deg)\tElevator(deg)\t";
    this->cStream << "Flap(-)\tSpeedBrake(-)\tSpoiler(-)\t";
    this->cStream << endl;
}

// 初始化函数
void B737Simulation::initialize(string resourceDir)
{
    // 飞行运动学模型
    this->cDOFModel.initialize(stepSize);

    // 大气环境模型
    this->cAtmosModel.input.altitude = cDOFModel.parameter.rLLA(AXIS_ALT);
    this->cAtmosModel.initialize();

    // 质量特性模型
    this->cMassModel.input.fuelExpendedL = 0.0;
    this->cMassModel.input.fuelExpendedR = 0.0;
    this->cMassModel.initialize();

    // 气动特性模型
    this->cAeroModel.initialize(resourceDir + "/B737");

    // 控制系统模型
    this->cControlModel.initialize(stepSize);

    // 动力系统模型 L
    this->cTurbineLModel.initialize(resourceDir + "/CFM56", stepSize);
    this->cTurbineLModel.starter(true);

    // 动力系统模型 R
    this->cTurbineRModel.initialize(resourceDir + "/CFM56", stepSize);
    this->cTurbineRModel.starter(true);

    // 起落架模型
    this->cLandingGearModel.initialize(stepSize);

    // 微分器
    this->cAlphaDot.initialize(Calculus::Derivative, Calculus::Euler, this->stepSize, 0.0);
    this->cBetaDot.initialize(Calculus::Derivative, Calculus::Euler, this->stepSize, 0.0);
}

// 单步运行函数
void B737Simulation::step(void)
{
    double v = 0.0, mach = 0.0;
    double alpha = 0.0, beta = 0.0;
    double alphaDot = 0.0, betaDot = 0.0;
    double Q, PR, DR, TR, DA;
    Vector force(3), torque(3), g(3);

    // 仿真运算循环
    while (currentTime < stopTime)
    {
        // 更新仿真时间
        currentTime = currentCount * stepSize;

        // 记录仿真数据
        this->cStream << currentTime << "\t";
        this->cStream << cDOFModel.output.rLLA << cDOFModel.output.vNav << cDOFModel.output.linearAccel;
        this->cStream << cDOFModel.output.eular << cDOFModel.output.omegaR;
        this->cStream << mach << "\t" << alpha << "\t" << beta << "\t";
        this->cStream << cLandingGearModel.output.force << cLandingGearModel.output.torque;
        this->cStream << cTurbineLModel.output.force + cTurbineRModel.output.force;
        this->cStream << cTurbineLModel.output.torque + cTurbineRModel.output.torque;
        this->cStream << cAeroModel.output.force << cAeroModel.output.torque;
        this->cStream << cControlModel.output.aileron << "\t" << cControlModel.output.rudder << "\t" << cControlModel.output.elevator << "\t";
        this->cStream << cControlModel.output.flap << "\t" << cControlModel.output.speedBrake << "\t" << cControlModel.output.spoiler << "\t";
        this->cStream << endl;

        // 飞行运动学模型
        force = cAeroModel.output.force + cLandingGearModel.output.force + cTurbineLModel.output.force + cTurbineRModel.output.force;
        torque = cAeroModel.output.torque + cLandingGearModel.output.torque + cTurbineLModel.output.torque + cTurbineRModel.output.torque;
        g = Geodesy::gravity(cDOFModel.output.rLLA(0), cDOFModel.output.rLLA(1), cDOFModel.output.rLLA(2));
        g = cDOFModel.output.cN2B * g * cMassModel.output.mass;

        this->cDOFModel.input.force = force;
        this->cDOFModel.input.torque = torque;
        this->cDOFModel.input.mass = cMassModel.output.mass;
        this->cDOFModel.input.inertia = cMassModel.output.inertia;
        this->cDOFModel.step();

        // 大气环境模型
        this->cAtmosModel.input.altitude = cDOFModel.output.rLLA(AXIS_ALT);
        this->cAtmosModel.step();

        // 飞行参数解算
        alpha = Coordinate::alpha(cDOFModel.output.vBody(AXIS_X), cDOFModel.output.vBody(AXIS_Y), cDOFModel.output.vBody(AXIS_Z));
        alphaDot = this->cAlphaDot.calculate(alpha);
        beta = Coordinate::beta(cDOFModel.output.vBody(AXIS_X), cDOFModel.output.vBody(AXIS_Y), cDOFModel.output.vBody(AXIS_Z));
        betaDot = this->cBetaDot.calculate(beta);
        v = Vector::norm(cDOFModel.output.vBody);
        mach = v / cAtmosModel.output.speed;
        Q = 0.5 * cAtmosModel.output.density * v * v;
        TR = cAtmosModel.output.temperature / Geodesy::temperature(0.0);
        DR = cAtmosModel.output.density / Geodesy::density(0.0);
        PR = cAtmosModel.output.pressure / Geodesy::pressure(0.0);
        DA = FT_TO_M * 145426.0 * (1.0 - pow(PR / TR, 0.235));

        // 更新操纵指令
        this->autopilot();

        // 质量特性模型
        this->cMassModel.input.fuelExpendedL = cTurbineLModel.output.fuelExpended;
        this->cMassModel.input.fuelExpendedR = cTurbineRModel.output.fuelExpended;
        this->cMassModel.step();

        // 起落架模型
        // this->cLandingGearModel.input.gearCommand = 0;
        // this->cLandingGearModel.input.steerCommand = 0;
        // this->cLandingGearModel.input.brakeCommand = 0;
        this->cLandingGearModel.input.vNav = cDOFModel.output.vNav;
        this->cLandingGearModel.input.eular = cDOFModel.output.eular;
        this->cLandingGearModel.input.omega = cDOFModel.output.omegaR;
        this->cLandingGearModel.input.altitudeGL = cDOFModel.output.rLLA(AXIS_ALT);
        this->cLandingGearModel.input.CG = cMassModel.output.CG;
        this->cLandingGearModel.step();

        // 动力系统模型 L
        // this->cTurbineLModel.input.throttle = 0;
        this->cTurbineLModel.input.mach = mach;
        this->cTurbineLModel.input.temperature = cAtmosModel.output.temperature;
        this->cTurbineLModel.input.dynamicPressure = Q;
        this->cTurbineLModel.input.densityRatio = DR;
        this->cTurbineLModel.input.densityAltitude = DA;
        this->cTurbineLModel.input.CG = cMassModel.output.CG;
        this->cTurbineLModel.step();

        // 动力系统模型 R
        // this->cTurbineLModel.input.throttle = 0;
        this->cTurbineRModel.input.mach = mach;
        this->cTurbineRModel.input.temperature = cAtmosModel.output.temperature;
        this->cTurbineRModel.input.dynamicPressure = Q;
        this->cTurbineRModel.input.densityRatio = DR;
        this->cTurbineRModel.input.densityAltitude = DA;
        this->cTurbineRModel.input.CG = cMassModel.output.CG;
        this->cTurbineRModel.step();

        // 气动特性模型
        this->cAeroModel.input.vTAS = v;
        this->cAeroModel.input.mach = mach;
        this->cAeroModel.input.alpha = alpha;
        this->cAeroModel.input.alphaDot = alphaDot;
        this->cAeroModel.input.beta = beta;
        this->cAeroModel.input.omega = cDOFModel.output.omegaR;
        this->cAeroModel.input.altitudeGL = cDOFModel.output.rLLA(AXIS_ALT);
        this->cAeroModel.input.dynamicPressure = Q;
        this->cAeroModel.input.CG = cMassModel.output.CG;

        this->cAeroModel.input.aileron = cAileronActutor.output.y;
        this->cAeroModel.input.rudder = cRudderActutor.output.y;
        this->cAeroModel.input.elevator = cElevatorActutor.output.y;
        this->cAeroModel.input.flap = cFlapActutor.output.y;
        this->cAeroModel.input.speedBrake = cSpeedActutor.output.y;
        this->cAeroModel.input.spoiler = cSpoilerActutor.output.y;
        this->cAeroModel.input.landingGear = cLandingGearModel.output.gearPosition;

        this->cAeroModel.input.aileron = cControlModel.output.aileron;
        this->cAeroModel.input.rudder = cControlModel.output.rudder;
        this->cAeroModel.input.elevator = cControlModel.output.elevator;
        this->cAeroModel.input.flap = cControlModel.output.flap;
        this->cAeroModel.input.speedBrake = cControlModel.output.speedBrake;
        this->cAeroModel.input.spoiler = cControlModel.output.spoiler;
        this->cAeroModel.input.landingGear = cLandingGearModel.output.gearPosition;

        this->cAeroModel.step();

        // 控制系统模型
        // this->cControlModel.input.aileronCommand = 0;
        // this->cControlModel.input.rudderCommand = 0;
        // this->cControlModel.input.elevatorCommand = 0;
        // this->cControlModel.input.flapCommand = 0;
        // this->cControlModel.input.speedBrakeCommand = 0;
        // this->cControlModel.input.spoilerCommand = 0;
        this->cControlModel.input.mach = mach;
        this->cControlModel.input.eular = cDOFModel.output.eular;
        this->cControlModel.input.omega = cDOFModel.output.omegaR;
        this->cControlModel.step();

        // 位置伺服执行机构
        this->cAileronActutor.input.u = cControlModel.output.aileron;
        this->cAileronActutor.step();

        this->cRudderActutor.input.u = cControlModel.output.rudder;
        this->cRudderActutor.step();

        this->cElevatorActutor.input.u = cControlModel.output.elevator;
        this->cElevatorActutor.step();

        this->cFlapActutor.input.u = cControlModel.output.flap;
        this->cFlapActutor.step();

        this->cSpeedActutor.input.u = cControlModel.output.speedBrake;
        this->cSpeedActutor.step();

        this->cSpoilerActutor.input.u = cControlModel.output.spoiler;
        this->cSpoilerActutor.step();

        // 更新周期计数器
        this->currentCount++;
    }

    // 关闭文件流
    this->cStream.close();
}

// 操纵指令函数
void B737Simulation::autopilot(void)
{
    double v;

    if (this->currentTime < 5.0)
    {
        this->cLandingGearModel.input.gearCommand = 1.0;
        this->cLandingGearModel.input.steerCommand = 0;
        this->cLandingGearModel.input.brakeCommand = 0;
        this->cTurbineLModel.input.throttle = 0;
        this->cTurbineRModel.input.throttle = 0;
        this->cControlModel.input.aileronCommand = 0;
        this->cControlModel.input.rudderCommand = 0;
        this->cControlModel.input.elevatorCommand = 0;
        this->cControlModel.input.speedBrakeCommand = 0;
        this->cControlModel.input.spoilerCommand = 0;
    }
    else if (this->currentTime < 22.5)
    {
        this->cTurbineLModel.input.throttle = 0.5;
        this->cTurbineRModel.input.throttle = 0.5;
    }
    else if (this->currentTime < 40.0)
    {
    }
    else if (this->currentTime < 50.0)
    {
        this->cLandingGearModel.input.steerCommand = 0.5;
    }
    else if (this->currentTime < 60.0)
    {
        this->cLandingGearModel.input.steerCommand = 0;
    }
    else
    {
        this->cLandingGearModel.input.brakeCommand = 1.0;
    }
}

// 构造函数
X24Simulation::X24Simulation()
{
}

// 析构函数
X24Simulation::~X24Simulation()
{
}

// 准备函数
void X24Simulation::setup(void)
{
    // 仿真求解配置
    startTime = 0.0;
    stopTime = 50.0;
    stepSize = 0.005;
    currentTime = 0.0;
    currentCount = 0;

    // 设置初始运动参数
    this->cDOFModel.parameter.rLLA(AXIS_LON) = -110.0;
    this->cDOFModel.parameter.rLLA(AXIS_LAT) = 47.0;
    this->cDOFModel.parameter.rLLA(AXIS_ALT) = 45000.0 * FT_TO_M;

    this->cDOFModel.parameter.vBody(AXIS_X) = 600.0 * FT_TO_M;
    this->cDOFModel.parameter.vBody(AXIS_Y) = -20 * FT_TO_M;
    this->cDOFModel.parameter.vBody(AXIS_Z) = 0;

    this->cDOFModel.parameter.eular(AXIS_X) = 0;
    this->cDOFModel.parameter.eular(AXIS_Y) = 90.0;
    this->cDOFModel.parameter.eular(AXIS_Z) = 0;

    this->cDOFModel.parameter.omega(AXIS_X) = 0;
    this->cDOFModel.parameter.omega(AXIS_Y) = 0;
    this->cDOFModel.parameter.omega(AXIS_Z) = 0;

    // 设置质量特性参数
    this->cMassModel.setup(AIRCRAFT_X24);

    // 设置起落架布局参数
    this->cLandingGearModel.setup(AIRCRAFT_X24);
    this->cLandingGearModel.parameter.initialPosition = 0.0;

    // 设置舵面机构参数
    this->cAileronActutor.parameter.k = 20.0;
    this->cAileronActutor.parameter.positionRange[AXIS_MIN] = -20.0;
    this->cAileronActutor.parameter.positionRange[AXIS_MAX] = +20.0;
    this->cAileronActutor.parameter.rateRange[AXIS_MIN] = -80.0;
    this->cAileronActutor.parameter.rateRange[AXIS_MAX] = +80.0;

    this->cRudderActutor.parameter.k = 20.0;
    this->cRudderActutor.parameter.positionRange[AXIS_MIN] = -30.0;
    this->cRudderActutor.parameter.positionRange[AXIS_MAX] = +30.0;
    this->cRudderActutor.parameter.rateRange[AXIS_MIN] = -80.0;
    this->cRudderActutor.parameter.rateRange[AXIS_MAX] = +80.0;

    this->cElevatorActutor.parameter.k = 20.0;
    this->cElevatorActutor.parameter.positionRange[AXIS_MIN] = -15.0;
    this->cElevatorActutor.parameter.positionRange[AXIS_MAX] = +35.0;
    this->cElevatorActutor.parameter.rateRange[AXIS_MIN] = -80.0;
    this->cElevatorActutor.parameter.rateRange[AXIS_MAX] = +80.0;

    // 创建本地数据文件
    this->cStream.open("Example8.txt");
    this->cStream.setf(ios::showpos);
    this->cStream.setf(ios::fixed);
    this->cStream.precision(8);

    // 打印数据标题文本
    this->cStream << "Time(s)\t";
    this->cStream << "Longitude(deg)\tLatitude(deg)\tAltitude(m)\t";
    this->cStream << "V-North(m/s)\tV-Up(m/s)\tV-East(m/s)\t";
    this->cStream << "A-Body-X(m/s^2)\tA-Body-Y(m/s^2)\tA-Body-Z(m/s^2)\t";
    this->cStream << "Gamma(deg)\tPsi(deg)\tZeta(deg)\t";
    this->cStream << "Omega-X(deg/s)\tOmega-Y(deg/s)\tOmega-Z(deg/s)\t";
    this->cStream << "Mach(-)\tAlpha(deg)\tBeta(deg)\t";
    this->cStream << "Gear-Force-X(N)\tGear-Force-Y(N)\tGear-Force-Z(N)\t";
    this->cStream << "Gear-Torque-X(N*m)\tGear-Torque-Y(N*m)\tGear-Torque-Z(N*m)\t";
    this->cStream << "Engine-Force-X(N)\tEngine-Force-Y(N)\tEngine-Force-Z(N)\t";
    this->cStream << "Engine-Torque-X(N*m)\tEngine-Torque-Y(N*m)\tEngine-Torque-Z(N*m)\t";
    this->cStream << "Aero-Force-X(N)\tAero-Force-Y(N)\tAero-Force-Z(N)\t";
    this->cStream << "Aero-Torque-X(N*m)\tAero-Torque-Y(N*m)\tAero-Torque-Z(N*m)\t";
    this->cStream << "Aileron(deg)\tRudder(deg)\tElevator(deg)\t";
    this->cStream << endl;
}

// 初始化函数
void X24Simulation::initialize(string resourceDir)
{
    // 飞行运动学模型
    this->cDOFModel.initialize(stepSize);

    // 大气环境模型
    this->cAtmosModel.input.altitude = cDOFModel.parameter.rLLA(AXIS_ALT);
    this->cAtmosModel.initialize();

    // 质量特性模型
    this->cMassModel.input.fuelExpendedL = 0.0;
    this->cMassModel.input.fuelExpendedR = 0.0;
    this->cMassModel.initialize();

    // 气动特性模型
    this->cAeroModel.initialize(resourceDir + "/X24");

    // 控制系统模型
    this->cControlModel.initialize(stepSize);

    // 动力系统模型
    this->cRocketModel.initialize(resourceDir + "/XLR99", stepSize);

    // 起落架模型
    this->cLandingGearModel.initialize(stepSize);
}

// 单步运行函数
void X24Simulation::step(void)
{
    double v = 0.0, mach = 0.0;
    double alpha = 0.0, beta = 0.0;
    double Q;
    Vector force(3), torque(3), g(3);

    // 仿真运算循环
    while (currentTime < stopTime)
    {
        // 更新仿真时间
        currentTime = currentCount * stepSize;

        // 记录仿真数据
        this->cStream << currentTime << "\t";
        this->cStream << cDOFModel.output.rLLA << cDOFModel.output.vNav << cDOFModel.output.linearAccel;
        this->cStream << cDOFModel.output.eular << cDOFModel.output.omegaR;
        this->cStream << mach << "\t" << alpha << "\t" << beta << "\t";
        this->cStream << cLandingGearModel.output.force << cLandingGearModel.output.torque;
        this->cStream << cRocketModel.output.force << cRocketModel.output.torque;
        this->cStream << cAeroModel.output.force << cAeroModel.output.torque;
        this->cStream << cControlModel.output.aileron << "\t" << cControlModel.output.rudder << "\t" << cControlModel.output.elevator << "\t";
        this->cStream << endl;

        // 飞行运动学模型
        force = cAeroModel.output.force + cLandingGearModel.output.force + cRocketModel.output.force;
        torque = cAeroModel.output.torque + cLandingGearModel.output.torque + cRocketModel.output.torque;
        g = Geodesy::gravity(cDOFModel.output.rLLA(0), cDOFModel.output.rLLA(1), cDOFModel.output.rLLA(2));
        g = cDOFModel.output.cN2B * g * cMassModel.output.mass;

        this->cDOFModel.input.force = force + g;
        this->cDOFModel.input.torque = torque;
        this->cDOFModel.input.mass = cMassModel.output.mass;
        this->cDOFModel.input.inertia = cMassModel.output.inertia;
        this->cDOFModel.step();

        // 大气环境模型
        this->cAtmosModel.input.altitude = cDOFModel.output.rLLA(AXIS_ALT);
        this->cAtmosModel.step();

        // 质量特性模型
        this->cMassModel.input.fuelExpendedL = cRocketModel.output.oxidizerExpended;
        this->cMassModel.input.fuelExpendedR = cRocketModel.output.fuelExpended;
        this->cMassModel.step();

        // 飞行参数解算
        alpha = Coordinate::alpha(cDOFModel.output.vBody(AXIS_X), cDOFModel.output.vBody(AXIS_Y), cDOFModel.output.vBody(AXIS_Z));
        beta = Coordinate::beta(cDOFModel.output.vBody(AXIS_X), cDOFModel.output.vBody(AXIS_Y), cDOFModel.output.vBody(AXIS_Z));
        v = Vector::norm(cDOFModel.output.vBody);
        mach = v / cAtmosModel.output.speed;
        Q = 0.5 * cAtmosModel.output.density * v * v;

        // 更新操纵指令
        this->autopilot();

        // 起落架模型
        // this->cLandingGearModel.input.gearCommand = 0;
        // this->cLandingGearModel.input.steerCommand = 0;
        // this->cLandingGearModel.input.brakeCommand = 0;
        this->cLandingGearModel.input.vNav = cDOFModel.output.vNav;
        this->cLandingGearModel.input.eular = cDOFModel.output.eular;
        this->cLandingGearModel.input.omega = cDOFModel.output.omegaR;
        this->cLandingGearModel.input.altitudeGL = cDOFModel.output.rLLA(AXIS_ALT);
        this->cLandingGearModel.input.CG = cMassModel.output.CG;
        this->cLandingGearModel.step();

        // 动力系统模型
        // this->cRocketModel.input.throttle = 0;
        this->cRocketModel.input.pressure = cAtmosModel.output.pressure;
        this->cRocketModel.input.CG = cMassModel.output.CG;
        this->cRocketModel.step();

        // 气动特性模型
        this->cAeroModel.input.vTAS = v;
        this->cAeroModel.input.mach = mach;
        this->cAeroModel.input.alpha = alpha;
        this->cAeroModel.input.beta = beta;
        this->cAeroModel.input.omega = cDOFModel.output.omegaR;
        this->cAeroModel.input.dynamicPressure = Q;
        this->cAeroModel.input.CG = cMassModel.output.CG;

        // 可自选是否使用执行机构
        // this->cAeroModel.input.aileron = cAileronActutor.output.y;
        // this->cAeroModel.input.rudder = cRudderActutor.output.y;
        // this->cAeroModel.input.elevator = cElevatorActutor.output.y;

        this->cAeroModel.input.aileron = cControlModel.output.aileron;
        this->cAeroModel.input.rudder = cControlModel.output.rudder;
        this->cAeroModel.input.elevator = cControlModel.output.elevator;

        this->cAeroModel.step();

        // 控制系统模型
        // this->cControlModel.input.aileronCommand = 0;
        // this->cControlModel.input.rudderCommand = 0;
        // this->cControlModel.input.elevatorCommand = 0;
        this->cControlModel.input.eular = cDOFModel.output.eular;
        this->cControlModel.input.omega = cDOFModel.output.omegaR;
        this->cControlModel.step();

        // 位置伺服执行机构
        this->cAileronActutor.input.u = cControlModel.output.aileron;
        this->cAileronActutor.step();

        this->cRudderActutor.input.u = cControlModel.output.rudder;
        this->cRudderActutor.step();

        this->cElevatorActutor.input.u = cControlModel.output.elevator;
        this->cElevatorActutor.step();

        // 更新周期计数器
        this->currentCount++;
    }

    // 关闭文件流
    this->cStream.close();
}

// 操纵指令函数
void X24Simulation::autopilot(void)
{
    double v;

    if (this->currentTime < 5.0)
    {
        this->cLandingGearModel.input.gearCommand = 0.0;
        this->cRocketModel.input.throttle = 0.0;
        this->cControlModel.input.aileronCommand = 0;
        this->cControlModel.input.rudderCommand = 0;
        this->cControlModel.input.elevatorCommand = 0;
    }
    else if (this->currentTime < 15.0)
    {
        this->cRocketModel.input.throttle = 1.0;
    }
    else if (this->currentTime < 22.5)
    {
        this->cControlModel.input.elevatorCommand = -0.1;
    }
    else
    {
        // this->cRocketModel.input.throttle = 1.0;
        // this->cControlModel.input.elevatorCommand = -0.1;
    }
}